{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os \n",
    "import sys"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4000/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4001/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4003/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4006/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4008/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4009/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4010/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4011/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4012/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4016/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4017/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4018/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4019/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4020/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4021/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4022/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4023/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4025/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4031/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4032/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4035/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4043/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4044/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4045/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4051/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4052/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4055/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4056/parts_render_after_merging/0.png': No such file or directory\n",
      "cp: cannot stat '/media/junting/SSD_data/mani_skill2_data/4024/parts_render_after_merging/0.png': No such file or directory\n"
     ]
    }
   ],
   "source": [
    "# copy one rendered image of all models to a folder\n",
    "model_dir = \"/media/junting/SSD_data/mani_skill2_data\"\n",
    "src_img = \"parts_render_after_merging/0.png\"\n",
    "target_dir = \"/media/junting/SSD_data/mani_skill2_data/model_thumbnails\"\n",
    "if not os.path.exists(target_dir):\n",
    "    os.mkdir(target_dir)\n",
    "\n",
    "# each model is a random 4-digit number, check format and list dir and sort\n",
    "model_list = [ item for item in os.listdir(model_dir) if item.isdigit()]\n",
    "for model in model_list:\n",
    "    src = os.path.join(model_dir, model, src_img)\n",
    "    dst = os.path.join(target_dir, model+\".png\")\n",
    "    os.system(\"cp {} {}\".format(src, dst))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "from scipy.spatial.transform import Rotation as R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "import numpy as np\n",
    "\n",
    "def flip_pose_matrix_with_point(original_pose_matrix, plane_normal, plane_point):\n",
    "    \"\"\"\n",
    "    Compute the flipped pose matrix with respect to a plane defined by a normal vector and a point on the plane.\n",
    "\n",
    "    Args:\n",
    "        original_pose_matrix (numpy.ndarray): The original pose matrix to be flipped.\n",
    "        plane_normal (numpy.ndarray): The normal vector of the plane.\n",
    "        plane_point (numpy.ndarray): A point on the plane.\n",
    "\n",
    "    Returns:\n",
    "        numpy.ndarray: The flipped pose matrix.\n",
    "    \"\"\"\n",
    "    # Ensure the plane normal is a unit vector\n",
    "    plane_normal = plane_normal / np.linalg.norm(plane_normal)\n",
    "\n",
    "    # Extract the rotation matrix and translation vector from the pose matrix\n",
    "    orientation_matrix = original_pose_matrix[:3, :3]\n",
    "    translation_vector = original_pose_matrix[:3, 3]\n",
    "\n",
    "    # Calculate the reflected position\n",
    "    reflected_position = translation_vector - 2 * np.dot(translation_vector - plane_point, plane_normal) * plane_normal\n",
    "\n",
    "    # Calculate the orthogonal projection of the rotation matrix onto the plane\n",
    "    projection = orientation_matrix - np.outer(plane_normal, plane_normal) @ orientation_matrix\n",
    "\n",
    "    # Calculate the reflected rotation matrix\n",
    "    reflected_orientation = -projection\n",
    "\n",
    "    flipped_pose_matrix = np.eye(4)\n",
    "    flipped_pose_matrix[:3, :3] = reflected_orientation\n",
    "    flipped_pose_matrix[:3, 3] = reflected_position\n",
    "\n",
    "    return flipped_pose_matrix\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "Original pose: \n",
    "  position: \n",
    "    x: 0.5849478244781494\n",
    "    y: -1.4837896823883057\n",
    "    z: 1.7380565404891968\n",
    "  orientation: \n",
    "    x: -0.2087007462978363\n",
    "    y: 0.09191908687353134\n",
    "    z: 0.8641879558563232\n",
    "    w: 0.4485242962837219\n",
    "\n",
    "\"\"\"\n",
    "# original pose\n",
    "position = np.array([0.5849478244781494, -1.4837896823883057, 1.7380565404891968])\n",
    "quaternion = np.array([-0.2087007462978363, 0.09191908687353134, 0.8641879558563232, 0.4485242962837219])\n",
    "pose_matrix = np.eye(4)\n",
    "pose_matrix[:3, 3] = position\n",
    "# pose_matrix[:3, :3] = R.from_quat(quaternion).as_matrix()\n",
    "pose_matrix[:3, :3] = R.from_euler('xyz', [0.0,  0.45913096,  2.17671885], degrees=False).as_matrix()\n",
    "\n",
    "# mirror plane\n",
    "mirror_normal = np.array([0, 1, 0])\n",
    "mirror_point = np.array([0, 0, 0])\n",
    "\n",
    "mirrored_pose_matrix = flip_pose_matrix_with_point(pose_matrix, mirror_normal, mirror_point)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "originnal position [ 0.58494782 -1.48378968  1.73805654]\n",
      "original euler:  [0.         0.45913096 2.17671885]\n",
      "mirrored position [0.58494782 1.48378968 1.73805654]\n",
      "mirrored euler:  [ 2.95282279 -0.50245442  0.70674306]\n"
     ]
    }
   ],
   "source": [
    "# convert original and mirrored quaternion to euler in radians\n",
    "original_euler = R.from_matrix(pose_matrix[:3, :3]).as_euler('xyz', degrees=False)\n",
    "mirrored_euler = R.from_matrix(mirrored_pose_matrix[:3, :3]).as_euler('xyz', degrees=False)\n",
    "print(\"originnal position\",  pose_matrix[:3, 3])\n",
    "print(\"original euler: \", original_euler)\n",
    "print(\"mirrored position\", mirrored_pose_matrix[:3, 3])\n",
    "print(\"mirrored euler: \", mirrored_euler)\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "franka",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.17"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
